#include "wrmnk.h"

namespace Jano {

WRMNK::WRMNK(double nk, double ndA, double ndB, double nlambda)
{
    m_k = nk;
    m_dA = ndA;
    m_dB = ndB;
    m_lambda = nlambda;
    m_vecTheta.resize(m_dA + m_dB +1);
    m_memoryU.resize(m_dB + m_k);
    //m_inifi = new ublas::vector<double>(m_dA+m_dB+1); //! Wektor regresyjny

    sizeX = 1 + m_dA + m_dB;

    epsilon = 0;
    beta = 100000;
    nextTheta = ublas::vector<double> (sizeX);
    previousTheta = ublas::vector<double> (sizeX);
    fi = ublas::vector<double> (sizeX);
    k = ublas::vector<double> (sizeX);
    I = ublas::identity_matrix<double> (sizeX);
    previousP = ublas::matrix<double> (sizeX,sizeX);
    nextP = ublas::matrix<double> (sizeX,sizeX);
    zero = ublas::matrix<double> (sizeX,sizeX);
    m = 0;

    tempSize = 1+m_dA+m_dB;
    numinator = ublas::matrix<double> (tempSize,tempSize);
    fraction = ublas::matrix<double> (tempSize,tempSize);


    zeroVec.resize(sizeX);
    for(int i=0; i<sizeX; i++){
        zeroVec[i]=0;
    }

    m_vecInitTheta = zeroVec;
}

WRMNK::~WRMNK(){
    //delete m_inifi;
}

/*! \brief Odwracanie macierzy
 *
 * Funkcja umożliwiająca odwrócenie macierzy
 *
 * \param input - macierz wejściowa do obrócenia
 * \param inverse - wynikowa macierz odwrócona
 * \return informacja o powodzeniu operacji
 */
template<class T>
bool WRMNK::InvertMatrix(const ublas::matrix<T>& input, ublas::matrix<T>& inverse)
{
        typedef ublas::permutation_matrix<std::size_t> pmatrix;
        // create a working copy of the input
        ublas::matrix<T> A(input);
        // create a permutation matrix for the LU-factorization
        pmatrix pm(A.size1());
        // perform LU-factorization
        int res = ublas::lu_factorize(A, pm);
        if (res != 0)
                return false;
        // create identity matrix of "inverse"
        inverse.assign(ublas::identity_matrix<T> (A.size1()));
        // backsubstitute to get the inverse
        ublas::lu_substitute(A, pm, inverse);
        return true;
}

/*! \brief Aktulizacja wektora regresyjnego
 *
 * Prywatna funkcja odpowiedzialna za aktualizację w każdym kroku identyfikacji
 * stanu wektora regresyjnego zawieracjącego próbki sygnału wejściowego i wyjściowego
 * identyfikowanego obiektu.
 *
 * \param u - próbka sygnału wejściowego
 * \param y - próbka sygnały wyjściowego obiektu
 * \return zaaktualizowany wektor regresyjny
 */
template<class T>
ublas::vector<T> WRMNK::updateFI(T u, T y, ublas::vector<T> fi){
    /*for(int i = (fi.size()-1); i > 1 ;i--)fi[i] = fi[i-1];

    fi[0] = y;
    fi[m_dA] = u;

    return fi;*/
    for(int i=m_memoryU.size()-1; i>=1; i--)m_memoryU[i] = m_memoryU[i-1];
    m_memoryU[0] = u;
    //cout << "memoryU = "<<m_memoryU<<endl;
    for(unsigned i = m_dA; i < fi.size() ;i++)
    {
        fi[i] = m_memoryU[m_k-1+i-m_dA];
    }
    if(m_dA!=0) fi[0] = y;
    //fi[dA] = u;

    return fi;
}

/*! \brief Funkcja konwertująca
 *
 * Prywatna funkcja umożliwiająca konwersię wektora z przestrzeni ublas
 * na wektor z przestrzeni std;
 *
 * \param ublasVec - wektor z przestrzeni ublas do konwersji
 * \return wektor z przestrzeni std
 */
template<class T>
std::vector<T> WRMNK::convertVectorUblasToSTD(ublas::vector<T> ublasVec){
    std::vector<T> stdVec;
    for(int i=0; i<static_cast<int>(ublasVec.size()); i++) stdVec.push_back(ublasVec[i]);
    return stdVec;
}

/*! \breif Algorytm RMNK z kierunkowym zapominaniem
 *
 * Funkcja składowa klasy realizująca algorytm RMNK z kierunkowym zapominaniem
 * w celu uniknięcia zjawiska zwanego wybuchem estymatora.
 *
 * \param nPreviousP - poprzednia macierz kowariancji
 * \param nFi - poprzedni wektor regresyjny
 * \return aktualna macierz kowariancji
 */
template<class T>
ublas::matrix<T> WRMNK::directionalForgeting(ublas::matrix<T> &nPreviousP, ublas::vector<T> &nFi){
    T sizeX = nFi.size();
    static T ki = 0, denominator=0.00001;
    ublas::matrix<T> tempMatrix(sizeX,sizeX), tempMatrix2(sizeX,sizeX);
    ublas::vector<T> tempVec(sizeX);

    //  RMNK Z KIERUNKOWYM ZAPOMINANIEM
    //Wyznaczenie wartości macierzy P^-1(i) z równania (32.19) str 525 ksiąski PSS D.Bismor
    InvertMatrix(nPreviousP,tempMatrix);
        //std::cout<<"P^-1(i-1) = "<<tempMatrix<<std::endl;
    tempMatrix2 = ublas::outer_prod(nFi,nFi);
        //std::cout<<"fi(i-1)*fT(i-1) = "<<tempMatrix2<<std::endl;
    tempMatrix = tempMatrix + tempMatrix2; // równanie (32.19)
        // std::cout<<"P~^-1(i) = "<<tempMatrix<<std::endl;

    //Wyznaczenie mianownika fiT(i-1)*P(i-1)*fi(i-1) z równania (32.21) str 525 ksiąski PSS D.Bismor
    tempVec = ublas::prod(nFi,nPreviousP); //fiT(i-1) * P(i-1)
        //std::cout <<"fiT(i-1) * P(i-1) = "<<tempVec<<std::endl;
    denominator = ublas::inner_prod(tempVec,nFi);//fiT(i-1) * P(i-1)*fi(i-1)
        //std::cout <<"fiT(i-1) * P(i-1)*fi(i-1) = "<<denominator<<std::endl;
    if(denominator == 0) denominator = 0.00001;
    //Dokładna wartość współczynnika k(i) z równania (32.19) str 525 ksiąski PSS D.Bismor
    ki = (1.0-m_lambda) * (1.0 + 1.0/denominator); // równanie (32.21)
    //Wyznaczenie wartości macierzy P^-1(i) z równania (32.20) str 525 ksiąski PSS D.Bismor
    tempMatrix2 = ublas::outer_prod(nFi,nFi);
    tempMatrix = tempMatrix - ki * tempMatrix2; // równanie (32.20)
    //Wyznaczenie wartości macierzy P(i) poprzez odwrócenie macierzy P^-1(i)
    InvertMatrix(tempMatrix,tempMatrix);

    return tempMatrix;
}


/*! \brief Identyfikacja WRMNK
 *
 * Funkcja umożliwiająca dokonanie identyfikacji obiektu zgodnie z
 * rekurencyjną ważoną metodą najmniejszych kwadratów
 *
 * \param objOutY - aktualna próbka sygnału wyjściowego identyfikowanego obiektu
 * \param regOutU - aktulana próbka sterowania pochodząca z regulatora będąca wejściem obiektu
 * \return Zidentyfikowane parametry w chwili i
 */
std::vector<double> WRMNK::identification(double objOutY, double regOutU, double gamma){
    // testowe
    //double  denominator = 0, lambda = getLambda();
    //ublas::vector<double> tempVec(tempSize);


    // DO IDENTYFIKACJI //

    //lambda = 0.99;

    if(m == 0){
        for(int i=0; i<sizeX; i++){
            for(int j=0; j<sizeX; j++){
                zero(i,j) = 0;
            }
        }
        previousP=zero;
        fi=zeroVec;
        m_vecTheta = m_vecInitTheta;
        previousTheta = m_vecInitTheta;
//        std::cout << "InitTheta: " << m_vecInitTheta << std::endl;
    }
    //std::cout<<fi<<std::endl;
    // DO IDENTYFIKACJI //
    //fi[0]=fi[1]=0;


        //std::cout<<"[y, u] = ["<<objOutY<<", "<<regOutU<<"]\n";

        /*static double sizeX = 2, epsilon = 0, beta = 10, denominator = 0;
        static ublas::vector<double> nextTheta(sizeX), previousTheta(sizeX), fi(sizeX), k(sizeX), tempVec(sizeX);
        static ublas::identity_matrix<double> tempI(sizeX);
        static ublas::matrix<double> previousP(sizeX,sizeX), nextP(sizeX,sizeX),numinator(sizeX, sizeX),fraction(sizeX,sizeX);*/
    if(m == 0){
        previousP = beta*I; // Warunek początkowy P(0) = beta * I
        //Wyznaczenie wektora wzmocnień
//        std::cout<<previousP<<std::endl;
//        std::cout<<fi<<std::endl;
        k = ublas::prod(previousP,fi);
//        std::cout<<"k(i) = P(i)*fi(i-1) = "<<k<<std::endl;
        epsilon = gamma - ublas::inner_prod(previousTheta,fi); //błąd predykcji jednokrokowej
//        std::cout<<"e(i) = y(i) - thetaT(i-1)*fi(i-1) = "<<epsilon<<std::endl;
        nextP = previousP;
        nextTheta = previousTheta + k * epsilon;
//        std::cout<<"prevTheta = " << previousTheta;
//        std::cout<<"theta(i) = theta(i-1) + k(i)*e(i) = "<<nextTheta<<std::endl;
        m++;
    }else{
        //std::cout<<"previousP = "<<previousP<<std::endl;
        //std::cout<<"previous fi = "<<fi<<std::endl;

         /*//TA CZĘŚĆ NIE UWZGLĘDNIA ZABEZPIECZENIA PRZED WYBUCHEM ESTYMATORA
        //Rozwiązywanie równania 32.8 z książki
        //Wyznaczenie licznika
        tempVec = ublas::prod(previousP, fi); //P(i-1)*fi(i-1)
            //std::cout <<"P(i-1)*fi(i-1) = "<<tempVec<<std::endl;
            //ublas::trans(tempVec);

            numinator = ublas::outer_prod(tempVec,fi); //P(i-1)*fi(i-1)*fiT(i-1)
            //numinator = ublas::prod(tempVec,fi);
            //std::cout <<"P(i-1)*fi(i-1)*fiT(i-1) = "<<numinator<<std::endl;
            numinator = ublas::prod(numinator,previousP); //P(i-1)*fi(i-1)*fiT(i-1)*P(i-1)
            //numinator = ublas::prod(tempVec,fi);
            //std::cout <<"P(i-1)*fi(i-1)*fiT(i-1)*P(i-1) = "<<numinator<<std::endl;

            //Wyznaczenie mianownika
            tempVec = ublas::prod(fi,previousP); //fiT(i-1) * P(i-1)
            //std::cout <<"fiT(i-1) * P(i-1) = "<<tempVec<<std::endl;

            denominator = ublas::inner_prod(tempVec,fi);//fiT(i-1) * P(i-1)*fi(i-1)
            //std::cout <<"fiT(i-1) * P(i-1)*fi(i-1) = "<<denominator<<std::endl;
            denominator = lambda + denominator;//lambda + fiT(i-1) * P(i-1)*fi(i-1)
            //std::cout <<"lambda + fiT(i-1) * P(i-1)*fi(i-1) = "<<denominator<<std::endl;

            //Wyznaczenie wartości ułamka
            fraction = numinator/denominator; //ułamek
            //std::cout<<"licznik/mianownik = "<<fraction<<std::endl;

            //Wyznaczenie kolejnej wartości P(i)
            nextP = (previousP - numinator/denominator)/1;
            //std::cout<<"P(i) = (32.8) ksiazka = "<<nextP<<std::endl;*/

         //WRMNK z regularyzacją macierzy kowariancji ale niestety coś tu nie działa więc olewka

        //Wyznaczenie wektora wzmocnień
        /*static ublas::matrix<double> tempMatrix(sizeX,sizeX), tempMatrix2(sizeX, sizeX);
         InvertMatrix(previousP,tempMatrix);
            //std::cout<<"P^-1(i-1) = "<<tempMatrix<<std::endl;

            tempMatrix2 = ublas::outer_prod(fi,fi);
            //std::cout<<"fi(i-1)*fT(i-1) = "<<tempMatrix2<<std::endl;

            tempMatrix = tempMatrix + tempMatrix2;
            //std::cout<<"P~^-1(i) = "<<tempMatrix<<std::endl;

            InvertMatrix(tempMatrix,tempMatrix);
            //std::cout<<"P~(i) = "<<tempMatrix<<std::endl;

            static double lambdaMAX = 2, lambdaMIN = 1;
            static double q = (lambdaMAX- lambdaMIN)/lambdaMAX;
            static double r = lambdaMIN;
            //std::cout<<"q = "<<q<<" r = "<<r<<std::endl;
            nextP = q * tempMatrix + r * I;*/
            //std::cout<<"P(i) = "<<nextP<<std::endl;

        /*  RMNK Z KIERUNKOWYM ZAPOMINANIEM
        InvertMatrix(previousP,tempMatrix);
        //std::cout<<"P^-1(i-1) = "<<tempMatrix<<std::endl;
        tempMatrix2 = ublas::outer_prod(fi,fi);
        //std::cout<<"fi(i-1)*fT(i-1) = "<<tempMatrix2<<std::endl;
        tempMatrix = tempMatrix + tempMatrix2;
        // std::cout<<"P~^-1(i) = "<<tempMatrix<<std::endl;

        //Wyznaczenie mianownika
        tempVec = ublas::prod(fi,previousP); //fiT(i-1) * P(i-1)
        //std::cout <<"fiT(i-1) * P(i-1) = "<<tempVec<<std::endl;
        denominator = ublas::inner_prod(tempVec,fi);//fiT(i-1) * P(i-1)*fi(i-1)
        //std::cout <<"fiT(i-1) * P(i-1)*fi(i-1) = "<<denominator<<std::endl;
        if(denominator == 0) denominator = 0.00001;
        ki = (1.0-m_lambda) * (1.0 + 1.0/denominator);
        tempMatrix2 = ublas::outer_prod(fi,fi);
        tempMatrix = tempMatrix - ki * tempMatrix2;
        InvertMatrix(tempMatrix,nextP);*/

        // KIERUNKOWE ZAPOMINANIE POWINNO ZAPOBIEC WYBUCHOWI
        nextP = directionalForgeting(previousP,fi);
        //std::cout<<nextP<<std::endl;

        k = ublas::prod(nextP,fi);
        //std::cout<<"k(i) = P(i)*fi(i-1) = "<<k<<std::endl;
        epsilon = gamma - ublas::inner_prod(previousTheta,fi); //błąd predykcji jednokrokowej
        //std::cout<<"e(i) = y(i) - thetaT(i-1)*fi(i-1) = "<<epsilon<<std::endl;

        nextTheta = previousTheta + k * epsilon;
        //std::cout<<"theta(i) = theta(i-1) + k(i)*e(i) = "<<nextTheta<<std::endl;
    }

    // Ten zabieg musi być zastosowany, gdyż w innym wypadku kumulacja błędów numerycznych
    // powoduje wybuch estymatora bez względu na zastosowaną metodą ochrony przed wybuchem
    if(fabs(objOutY) < 0.00000001)gamma=0;  //[M]
    //std::cout<<fi<<std::endl;
    fi = updateFI(regOutU,-objOutY,fi);  // [M]

    previousP = nextP;
    previousTheta = nextTheta;
    m_vecTheta = nextTheta;

    return convertVectorUblasToSTD(m_vecTheta);
}
/*! \brief Odczytanie zidentyfikowanych parametrów
 *
 * Funkcja umożliwia odczyt parametrów identyfikacji
 *
 * \param factors - określa który z wielomianół chcemy odczytać
 * \return wybrany wielomian
 */
std::vector<double> WRMNK::getPolynomialFactors(std::string factors){
        std::vector<double> param;
        if(!factors.compare("wielomianA")){
                param.resize(m_dA+1,0.0);
                param[0]=1.0;
                for (unsigned int i=0;i<m_dA;i++) {
                        param[i+1]=m_vecTheta[i];
                }

        }else if(!factors.compare("wielomianB")){
                param.resize(m_dB+1,0.0);
                for (unsigned int i=0;i<m_dB+1;i++) {
                        param[i]=m_vecTheta[i + m_dA];
                }

        }

return param;
}

std::vector<double> WRMNK::getAPoly()
{
    std::vector<double> param;

    param.resize(m_dA + 1, 0.0);
    param[0] = 1.0;

    for(int i = 0; i<m_dA; i++) {
        param[i+1] = m_vecTheta[i];
    }

    return param;
}

std::vector<double> WRMNK::getBPoly()
{
    std::vector<double> param;

    param.resize(m_dB + 1, 0.0);
    for (unsigned int i=0; i < m_dB+1; i++) {
        param[i]=m_vecTheta[i + m_dA];
    }

    return param;
}

void WRMNK::setInitVal(const std::vector<double>& initA, const std::vector<double>& initB)
{
    for(int i = 0; i<m_dA; i++) {
        m_vecInitTheta[i] = initA[i+1];
    }

    for (unsigned int i=0; i < m_dB+1; i++) {
        m_vecInitTheta[i + m_dA] = initB[i];
    }
}

/*! \brief Ustawianie parametrów
 *
 * Procedura umożliwiająca ustawienie konfiguracji identyfikowanego obiektu.
 *
 * \param nk - opóźnienie obiektu
 * \param ndA - stopień wielomianu A(z^-1)
 * \param ndB - stopień wielomianu B(z^-1)
 * \param nlambda - współczynnik zapominania
 */
void WRMNK::setStructure(double nk, double ndA, double ndB, double nlambda){
    m_k = nk;
    m_dA = ndA;
    m_dB = ndB;
    m_lambda = nlambda;
}

/*! \brief Pobranie parametru k
 *
 * \return opóźnienie identyfikowanego obiektu
 */
double WRMNK::getK(){
    return m_k;
}

/*! \brief Pobranie stopnia wielomianu A
 *
 * \return stopień wielomianu A identyfikowanego obiektu
 */
double WRMNK::getdA(){
    return m_dA;
}

/*! \brief Pobranie stopnia wielomianu B
 *
 * \return stopień wielomianu B identyfikowanego obiektu
 */
double WRMNK::getdB(){
    return m_dB;
}
/*! \brief Pobranie współczynnika zapominania algorytmu
 *
 * \return współczynnik zapominania
 */
double WRMNK::getLambda(){
    return m_lambda;
}

} // namespace Jano











